/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/include/agvNavigate/publicInterfaceInc/public_macro.h
 * @Description  : 定义了AGV导航系统中的各种常量、枚举、结构体和控制信号，用于处理AGV的位姿、控制状态和任务错误等信息。  
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 16:25:53
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/  
#pragma once
#include "rclcpp/rclcpp.hpp"
#include "timestamp.h"
#include <agv_msgs/msg/pose_with_confidence.hpp>
#include <agv_msgs/msg/confidence_level.hpp>
#include <agv_msgs/msg/read_in_puts_new.hpp>
#include <agv_msgs/msg/read_in_puts_analog.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <agv_msgs/msg/periph_status.hpp>
#include <agv_msgs/msg/odometry_lite.hpp>
#include <agv_msgs/msg/recognition_result.hpp>
constexpr int AGV_NAV_NODE_ID = 100;    // 当前节点的ID标识
constexpr const char* ACTION_SOURCE = "local";

enum class AGV_TASK_ERROR {
    ERROR_CLEAT = -1,
    ERROR_OP_ERROR = 14, // 与事件中的数字相同 切勿更改
    ERROR_TASK_COMPILE_ERROR = 2,
    ERROR_PATH_LOST = 11
};

enum class AgvWorkMode {
    mod_auto_offline = 0,
    mod_manual = 1,
    mod_semiauto = 2,
    mod_auto_online = 3
};

struct CurAgvPose {
    std::mutex pose_mutex; // 位姿态更新锁
    agv_msgs::msg::PoseWithConfidence real_pose; // 当前导航模式下的位姿数据(二维码+激光)
    DataTimeStamp<agv_msgs::msg::ConfidenceLevel> laser_score; // 激光导航评分(二维码到激光路段切换前，需要判断此评分是否可靠)
    bool is_valid  = false;
    CurAgvPose() {
        real_pose.pose.pose.position.x = -8888;
        real_pose.pose.pose.position.y = -8888;
    }
};

// 立即小车控制结构(来自于调度系统立即动作)
struct InstantControl {
    bool virtual_load = false;
    InstantControl() = default;
};

// 停止信号类别
enum class StopType {
    STOP_RELOCATE = 0,
    STOP_START_DELAY = 1
};

enum class DoSteerState {
    SteerNormal = 0,
    SteerError = 1,
    Steering = 2,
    SteerBrakeLock = 3 //抱闸锁住
};

// 其他可设置控制车辆急停标识结构
struct OtherStopSignal {
    bool relocate_stop = false;
    bool start_delay = false;
    OtherStopSignal() = default;
};

// 本地op动作订阅使用的话题数据
struct OpUseTopic {
    agv_msgs::msg::OdometryLite odom_msg;                    // 当前里程计反馈
    agv_msgs::msg::PeriphStatus periph_status_msg;           // 订阅的外设信息,包括有无货物
    agv_msgs::msg::ReadInPutsAnalog input_analog_msg;        // 模拟量输入信息
    DataTimeStamp<agv_msgs::msg::ReadInPutsNew> input_new_io_msg;  // 输入IO消息
    geometry_msgs::msg::PoseStamped ground_camera_pose_msg;  // 下读头相机数据
    geometry_msgs::msg::PoseStamped rack_camera_pose_msg;    // 上读头相机数据
    DataTimeStamp<agv_msgs::msg::RecognitionResult> pallet_result_msg; // 栈板识别结果
};

// 空闲时,起步发布闪烁标识
struct IdleToRun {
    DataTimeStamp<bool> idle_status;
    TimerTriggerOnce timer;
    bool to_blink = false;

    IdleToRun() {
        Reset();
    }

    void Reset() {
        timer.Stop();
        idle_status.Update(true);
        to_blink = false;
        DEBUG_OUT("IdleToRun reset once");
    }

    bool Update(bool current_idle, int idle_time_ms, int blink_time_ms) {
        bool last_idle = idle_status.Get();
        bool idle_time_ok = idle_status.TimeOut(idle_time_ms);

        if (last_idle != current_idle) {
            if (last_idle && idle_time_ok) {
                timer.Stop();
                to_blink = true;
                DEBUG_OUT("IdleToRun update once; set blink ok");
            }
        }

        if (to_blink) {
            timer.Start();
            if (timer.TriggerAlways(blink_time_ms)) {
                to_blink = false;
                DEBUG_OUT("toBlink set close; blinkTimeMs=" << blink_time_ms);
            } else {
                DEBUG_OUT("toBlink set open; blinkTimeMs=" << blink_time_ms);
            }
        }

        if (last_idle != current_idle) {
            idle_status.Update(current_idle);
            DEBUG_OUT("IdleToRun update once; lastIdle=" << last_idle << ", curIdle=" << current_idle);
        }
        return to_blink;
    }
};